﻿// - Christopher J Hammond http://www.chrishammond.com 
// this code is adapted from the following thread on netduino.com 
// http://forums.netduino.com/index.php?/topic/780-netduino-with-adafruit-motor-shield/page__view__findpost__p__8045
// the code I've put together is released under the Ms-PL license (http://christocnetduino.codeplex.com/license), code from other authors was found in the public domain

//
// version 1.0.0.0 | simple forward/back movement in 3 second intervals.
// 

using System;
using System.Threading;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;

namespace com.christoc.netduino.MotorTest
{
    public class Program
    {
        //get an instance of the DcMotor class passing in the enumerated header M4 (netduino can access M3 and M4 on the adafruit motor shield)
        static DcMotor rightDrive = new DcMotor(MotorHeaders.M3);
        static DcMotor leftDrive = new DcMotor(MotorHeaders.M4);


        private enum runprograms
        {
            One,
            Two,
            Three,
            Four
        } ;


        static int _curProgram = (int)runprograms.One;

        private const long DebounceDelay = 10000000;


        private static long _lastButtonPushed;
        public static void Main()
        {

            var switchButton = new InterruptPort(Pins.GPIO_PIN_A1, true, Port.ResistorMode.PullUp,
                                                 Port.InterruptMode.InterruptEdgeLow);

            switchButton.OnInterrupt += switchButton_OnInterrupt;
            //set the speed to 0 to start
            rightDrive.SetSpeed(0);
            leftDrive.SetSpeed(0);

            //set the direction of the motor
            rightDrive.Run(MotorDirection.Forward);
            leftDrive.Run(MotorDirection.Forward);

            while (true)
            {
                RunCurrentProgram();
            }
            // ReSharper disable FunctionNeverReturns
        }
        // ReSharper restore FunctionNeverReturns

        static private void RunCurrentProgram()
        {
            switch (_curProgram)
            {
                case 0:
                    RunProgram1();
                    
                    break;
                case 1:
                    RunProgram2();
                    
                    break;
                case 2:
                    RunProgram3();
                    
                    break;
                case 3:
                    Stop();
                    break;
                default: RunProgram1();
                    
                    break;
            }
        }

        private static void RunProgram1()
        {
            Spin(100, 2000, true);
            Stop();
            GoStraight(100, 2000, MotorDirection.Forward);
            Spin(100, 2000, false);
            GoStraight(100, 2000, MotorDirection.Reverse);
        }

        private static void RunProgram2()
        {
            Spin(100, 3000, true);
        }

        private static void RunProgram3()
        {
            GoStraight(100, 2000, MotorDirection.Forward);
            Stop();
            GoStraight(100, 2000, MotorDirection.Reverse);
            Stop();
        }
        //method to spin in a circle clockwise at a speed for a length of time
        static private void Spin(uint speed, int lot, bool direction)
        {
            rightDrive.SetSpeed(speed);
            leftDrive.SetSpeed(speed);
            if (direction)
            {
                rightDrive.Run(MotorDirection.Forward);
                leftDrive.Run(MotorDirection.Reverse);
            }
            else
            {
                rightDrive.Run(MotorDirection.Reverse);
                leftDrive.Run(MotorDirection.Forward);
            }
            Thread.Sleep(lot);
        }

        private static void Stop()
        {
            rightDrive.SetSpeed(0);
            leftDrive.SetSpeed(0);
        }

        private static void GoStraight(uint speed, int lot, MotorDirection direction)
        {
            rightDrive.Run(direction);
            leftDrive.Run(direction);
            rightDrive.SetSpeed(speed);
            leftDrive.SetSpeed(speed);
            Thread.Sleep(lot);
        }

        private static void switchButton_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            if ((DateTime.Now.Ticks - _lastButtonPushed) > DebounceDelay)
            {
                _lastButtonPushed = DateTime.Now.Ticks;
                if (_curProgram > 2)
                    _curProgram = 0;
                else
                {
                    _curProgram++;
                }
            }
        }
    }
}
